close all;
clear;


id = 4;
is_single = true;
dat_path = sprintf('datas/%02d/resamp_verify_%02d.dat', id, id);
param_path = sprintf("./datas/%02d/fcn_%02d.bin", id, id);
rpc_net = loadRPCnet(param_path);

[vgcp_x, vgcp_y, vgcp_z, vgcp_col, vgcp_row, img_size] = loadResampDat(dat_path);

if is_single
    vgcp_x = single(vgcp_x);
    vgcp_y = single(vgcp_y);
    vgcp_z = single(vgcp_z);
    vgcp_col = single(vgcp_col);
    vgcp_row = single(vgcp_row);
end


[est_col, est_row] = rpcNetPredict(rpc_net, vgcp_x, vgcp_y, vgcp_z);
error_col_svd = est_col - vgcp_col;
error_row_svd = est_row - vgcp_row;

figure;
subplot(211);
plot(error_col_svd);
title("net error x");
grid on;
subplot(212);
plot(error_row_svd);
title("net error y");
grid on;

%%
function rpc_net = loadRPCnet(param_path)

    fid = fopen(param_path, "rb");
    if fid == -1
        error("cannot open: %s\n", param_path);
    end
    params = fread(fid, 108, "float");
    fclose(fid);
    
    rpc_net.m_longScale = params(1);
    rpc_net.m_latScale = params(2);
    rpc_net.m_heightScale = params(3);
    rpc_net.m_sampScale = params(4);
    rpc_net.m_lineScale = params(5);
    
    rpc_net.m_longOffset = params(6);
    rpc_net.m_latOffset = params(7);
    rpc_net.m_heightOffset = params(8);
    rpc_net.m_sampOffset = params(9);
    rpc_net.m_lineOffset = params(10);
    
    rpc_net.m_weight0 = params(10 + (1:48));
    rpc_net.m_weight0 = reshape(rpc_net.m_weight0, [3, 16])';
    rpc_net.m_bias0 = params(10 + (49:64));
    rpc_net.m_bias0 = rpc_net.m_bias0(:);
    rpc_net.m_weight1 = params(10 + (65:96));
    rpc_net.m_weight1 = reshape(rpc_net.m_weight1, [16, 2])';
    rpc_net.m_bias1 = params(10 + (97:98));
    rpc_net.m_bias1 = rpc_net.m_bias1(:);

end

function [vgcp_col, vgcp_row] = rpcNetPredict(rpc_net, vgcp_x, vgcp_y, vgcp_z)
    
    vgcp_x = (vgcp_x - rpc_net.m_longOffset) / rpc_net.m_longScale;
    vgcp_y = (vgcp_y - rpc_net.m_latOffset) / rpc_net.m_latScale;
    vgcp_z = (vgcp_z - rpc_net.m_heightOffset) / rpc_net.m_heightScale;
    
    X = [vgcp_x, vgcp_y, vgcp_z].';
    y1 = rpc_net.m_weight0*X + rpc_net.m_bias0;
    y1 = max(y1, 0);
    y2 = rpc_net.m_weight1*y1 + rpc_net.m_bias1;
    
    vgcp_col = y2(1,:).';
    vgcp_row = y2(2,:).';

    vgcp_col = vgcp_col*rpc_net.m_sampScale + rpc_net.m_sampOffset;
    vgcp_row = vgcp_row*rpc_net.m_lineScale + rpc_net.m_lineOffset;
end
